/*
 * LSI Stand
 * version 4, final version
 * Code originally developed by the STA Capstone team at UMBC, Spring 2013
 * Code modified by the LSI Capstone team at UMBC, Spring 2014
 */
 
#include <StandInstance.h>
#include <StepperDriver.h>
#include <Decoder.h>
#include <string.h>

#define LED 13

// initialize the stand position to 0 deg az, 0 deg el
// in Stand, reset position is set to 0,0
StandInstance stand(0,0); // Declare stand instance

// Declare global variables
int MAX_COMMAND_SIZE = 10;
int MAX_LATLONG_SIZE = 6;
int MAX_NUM_PARAMS = 3;
int blinkOnDuration = 100;
int blinkPeriod = 3900;
long timeOfLastBlink = 0;

void setup()
{
  Serial.begin(9600);
  // set LED pin to output
  pinMode(LED,OUTPUT);
}

/*
 * Wait for commands on serial. Command are:
 *    57           Connection request from Matlab. Reply with "1"
 *    move 271 4   Move to 271 deg az, 4 deg el
 *    stepaz 600   Step 600 steps Clockwise in azimuth
 *    stepel -300  Step 300 steps CCW in elevation
 *    reset        Move stand to reset position
 */
void loop()
{
  if (Serial.available()) {
     int positionCnt = 0;  
     char parm1[MAX_COMMAND_SIZE], parm2[MAX_COMMAND_SIZE], parm3[MAX_COMMAND_SIZE];  // Declare arrays to store parsed params

     char buffer[MAX_COMMAND_SIZE * MAX_NUM_PARAMS + MAX_NUM_PARAMS];  // Declare buffer to receive serial data
     int tempLen = Serial.readBytesUntil('\n',buffer,MAX_COMMAND_SIZE * MAX_NUM_PARAMS + MAX_NUM_PARAMS); // Determines number of characters read into buffer
     
     char command[tempLen + 1]; // Initialize array to store command
     
     //convert to char array
     int i;
     for( i = 0; i < tempLen; i++) {  // Transfer command from buffer into command array
       command[i] = buffer[i];
     }
     command[i + 1] = '\0';  

     // Parse first word in command
     for(i = 0; i < tempLen && command[positionCnt] != ' ' && command[positionCnt] != '\0'; i++) {
       parm1[i] = command[positionCnt];
       positionCnt++;
     }
     parm1[i] = '\0';
     positionCnt++;
     
     // "move" command received
     if(strcmp(parm1,"move") == 0) {
       //get azimuth degree value (parse second command )
       for(i = 0; i < (tempLen - positionCnt) && command[positionCnt] != ' '; i++) {
        parm2[i] = command[positionCnt];
        positionCnt++;
       } 
       parm2[i] = '\0';
       positionCnt++;

       //get elevation degree value
       for(i = 0; i < (tempLen - positionCnt + 1) && command[positionCnt] != ' ' && command[positionCnt] != '\0'; i++) {
        parm3[i] = command[positionCnt];
        positionCnt++;
       }
       parm3[i] = '\0';
       positionCnt++;
       
       Serial.print("move ");
       Serial.print(parm2);
       Serial.print(" ");
       Serial.println(parm3);
       move(atoi(parm2), atoi(parm3));       
     }
     // manual step control in azimuth. format: "stepaz 200" (200 steps)
     else if(strcmp(parm1, "stepaz") == 0) {
       for(i = 0; positionCnt < tempLen && command[positionCnt] != ' ' && command[positionCnt] != '\0'; i++) {
        parm2[i] = command[positionCnt];
        positionCnt++;
       } 
       parm2[i] = '\0';
       positionCnt++;
       
       Serial.print("stepaz ");
       Serial.print(parm2);
       Serial.print(" ; atol(parm2) = ");
       Serial.println(atol(parm2));
       stepAz(atol(parm2));
     }
     // manual step control in elevation. format: "stepel 200" (200 steps)
     else if(strcmp(parm1, "stepel") == 0) {
       for(i = 0; positionCnt < tempLen && command[positionCnt] != ' ' && command[positionCnt] != '\0'; i++) {
        parm2[i] = command[positionCnt];
        positionCnt++;
       }
       parm2[i] = '\0';
       positionCnt++;

       Serial.print("stepel ");
       Serial.print(parm2);
       Serial.print(" ; atol(parm2) = ");
       Serial.println(atol(parm2));
       stepEl(atol(parm2));
     }
     // "reset" command received
     else if(strcmp(parm1, "reset") == 0) {
       reset();
     }
     // connection setup request
     else if(strcmp(parm1, "57") == 0) {
       Serial.println(1);
       digitalWrite(13,HIGH);
       delay(300);
       digitalWrite(13,LOW);
       delay(200);
       digitalWrite(13,HIGH);
       delay(300);
       digitalWrite(13,LOW);
       delay(200);
       digitalWrite(13,HIGH);
       delay(500);
       digitalWrite(13,LOW);
     }
     // command not recognized
     else {
       Serial.print(parm1);
       Serial.println(": is not a valid command."); 
     }
  }

  
  // blink the LED to indicate running status
  if (millis() - timeOfLastBlink > blinkPeriod) {
    digitalWrite(LED,HIGH);
    delay(blinkOnDuration);
    digitalWrite(LED,LOW);
    timeOfLastBlink = millis();
  }
  delay(100);
}


void move(int az, int el)
{
  digitalWrite(LED,HIGH);
  stand.moveToAccel(az,el);
  digitalWrite(LED,LOW);
  delay(100);
  printPosition();
}

/*
 *  steps is in 1/32 micro-steps
 */
void stepAz(long steps)
{
  stand.stepAzimuth(steps);
  delay(100);
}

/*
 *  steps is in 1/32 micro-steps
 */
void stepEl(long steps)
{
  stand.stepElevation(steps);
  delay(100);
}

void reset()
{
  Serial.println("Moving to reset position");
  stand.resetPosition();
  printPosition();
}

void printPosition()
{
  Serial.print("Status: Azimuth=");
  Serial.print(stand.getAzimuth());
  Serial.print(" Elevation=");
  Serial.println(stand.getElevation());
}
